'''
Created on Mar 9, 2011

@author: Donald May, Blayne Henke, and Andy Kroll
'''

from visual import *
import ode;
from QuadRotor import *

class QuadRotorApp(object):
    def __init__(self):
        self.init_scene()
        self.running = False
        self.should_exit = False
        
    def init_scene(self):
        self.quad_scene = display(title = "Quadrotor Simulation",
                                  x = 0, y = 0, width = 800, height = 600, 
                                  background = color.cyan)
        self.quad_scene.autocenter = 0
        self.quad_scene.autoscale = 1
        self.quad_scene.select()
        
        self.instruction_text = label(pos = (0, 0, 0), yoffset = 0, height = 20, text = "Welcome to the Quadrotor Simulation!", box = False, line = False)
        self.instruction_text2= label(pos = (0, 0, 0), yoffset = -25,height = 20,  text = "A increases throttle, S decreases throttle", box = False, line = False)
        self.instruction_text3 = label(pos = (0, 0, 0), yoffset = -65, height = 20,  text = "Simulation is stopped while this text is visible!", box = False, line = False)
        
        self.world = ode.World()
        self.gravity = (0, -9.81, 0)
        self.world.setGravity(self.gravity)
        self.world.setERP(0.8)
        self.world.setCFM(1E-5)
        
        self.thrust = 0
        self.yaw = 0
        
        self.space = ode.Space()
        
        self.ground_plane = box(pos = (0, -14, 0), axis = (1,0,0),
                                length = 160, width = 160, height = 0.5)
        
        self.ground_plane.color = color.green
        
        self.floor = ode.GeomPlane(self.space, (0, 1, 0), -13.5)

        self.contact_group = ode.JointGroup()
        
        self.quad_scene.autoscale = 0
        

        
        self.quad_rotor = QuadRotor(self.world, self.space)
        
    def get_scene(self):
        return self.quad_scene
    
    def get_gravity(self):
        return self.gravity
    
    def set_gravity(self, gravity):
        self.gravity = gravity;
        self.world.setGravity(self.gravity)
        
    def set_thrust(self, thrust):
        self.thrust = thrust
        self.quad_rotor.set_thrust(self.thrust)
        
    def set_yaw(self, yaw):
        self.yaw = yaw
        self.quad_rotor.set_yaw(yaw)
        
    def set_motor_dir(self, motor_num, motor_dir):
        self.quad_rotor.set_motor_dir(motor_num, motor_dir) 
        
    def get_motor_dir(self, motor_num):
        return self.quad_rotor.get_motor_dir(motor_num)   
    
    def collide_callback(self, args, geom1,geom2):
        contacts = ode.collide(geom1, geom2)
        world,contactgroup = args;
        
        for cur_contact in contacts:
            cur_contact.setBounce(0.01)
            cur_contact.setMu(5000)
            j = ode.ContactJoint(world,contactgroup,cur_contact)
            j.attach(geom1.getBody(), geom2.getBody())
            
    def start(self):
        self.running = True
        self.instruction_text.visible = False
        self.instruction_text2.visible = False
        self.instruction_text3.visible = False
        
    def stop(self):
        self.running = False
        self.instruction_text.visible = True
        self.instruction_text2.visible = True
        self.instruction_text3.visible = True
        
    def reset(self):
        self.quad_rotor.reset()
        
    def quit_app(self):
        self.should_exit = True
        self.running = False
        ode.CloseODE()
        self.quad_scene.visible = False
        
    def update(self, fps):
        if self.running:
            self.space.collide((self.world, self.contact_group), self.collide_callback)
            self.world.step(1.0 / fps)    
            self.quad_rotor.update()
            self.contact_group.empty()
           
            
            